classdef KinectController
    %KINECTCONTROLLER Controller class for Kinect COM Interface.
       
    properties
        m_AssemblyInfo;
    end
    
    properties(SetAccess = private)
        m_KinectObject;
    end
    
       
    methods
        function object = KinectController()
           dir = pwd();
           object.m_AssemblyInfo = NET.addAssembly(strcat(dir, '/Kinect_COM.dll'));
           object.m_KinectObject = Kinect_COM.KinectInterface();
        end
        
        %Starts Kinect        
        function object = StartKinect(object, colorStreamType, depthStreamType)
            object.m_KinectObject.StartKinect(7, colorStreamType, depthStreamType);
        end
        
        %Stops Kinect
        function object = StopKinect(object)
            object.m_KinectObject.StopKinect();
        end
        
        %Retrieves Color Image Data From buffer
        function imageData = GetColorImageData(object)
            rawData = object.m_KinectObject.GetColorData();
            imageData = KinectController.ProcessColorDataRGB(rawData, ... 
                                                        object.m_KinectObject.GetColorStreamWidth(), ... 
                                                        object.m_KinectObject.GetColorStreamHeight());
        end
        
        %Retrieves Depth Image Data From buffer
        function imageData = GetDepthImageData(object)
            rawData = object.m_KinectObject.GetDepthData();
            imageData = KinectController.ProcessDepthData(rawData, ... 
                                                        object.m_KinectObject.GetDepthStreamWidth(), ... 
                                                        object.m_KinectObject.GetDepthStreamHeight());
        end
        
        %Modifies the elevation angle
        function ModifyElevationAngle(object, angle)
            object.m_KinectObject.SetElevationangle(angle);
        end
    end
    
    methods(Static)
        %Converts raw color data into matrix
        %For RGB Color Format
        function imageData = ProcessColorDataRGB(rawData, width, height)
            data = uint8(rawData);
            bytesPerPixel = size(data, 2) / (width * height);
            imageData = zeros(height, width,(3), 'uint8');
            for i = 1:height
                for j = 1:width
                    colorOffset = bytesPerPixel;
                    while(colorOffset > 1)
                        imageData(i, j, bytesPerPixel - colorOffset + 1) = data((width * (i - 1) + (j - 1)) * bytesPerPixel + colorOffset - 1);
                        colorOffset = colorOffset - 1;
                    end
                end
            end
        end
        
        %Converts raw depth data into matrix
        function imageData = ProcessDepthData(rawData, width, height)
            data = uint8(rawData);
            imageData = zeros(height, width, 'uint8');
            for i = 1:height
                for j = 1:width
                    imageData(i, j) = data((width * (i - 1) + (j - 1)) + 1);
                end
            end
        end
    end
                        
    
end

